
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_frame.c
  * @author     baiyang
  * @date       2022-11-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_frame.h"

#include "sitl.h"

#include <stdio.h>
#include <string.h>

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/
#define AP_MOTORS_MATRIX_YAW_FACTOR_CW   -1
#define AP_MOTORS_MATRIX_YAW_FACTOR_CCW   1

#define AP_MOTORS_MOT_1 0U
#define AP_MOTORS_MOT_2 1U
#define AP_MOTORS_MOT_3 2U
#define AP_MOTORS_MOT_4 3U
#define AP_MOTORS_MOT_5 4U
#define AP_MOTORS_MOT_6 5U
#define AP_MOTORS_MOT_7 6U
#define AP_MOTORS_MOT_8 7U
#define AP_MOTORS_MOT_9 8U
#define AP_MOTORS_MOT_10 9U
#define AP_MOTORS_MOT_11 10U
#define AP_MOTORS_MOT_12 11U

#define DEF_MOTOR(_servo, _angle, _yaw_factor, _display_order) {\
        .servo = _servo, \
        .angle = _angle, \
        .yaw_factor = _yaw_factor, \
        .display_order = _display_order \
}

#define DEF_MOTOR2(_servo, _angle, _yaw_factor, _display_order, _roll_servo, _roll_min, _roll_max, _pitch_servo, _pitch_min, _pitch_max) {\
        .servo = _servo, \
        .angle = _angle, \
        .yaw_factor = _yaw_factor, \
        .display_order = _display_order, \
        .roll_servo = _roll_servo, \
        .roll_min = _roll_min, \
        .roll_max = _roll_max, \
        .pitch_servo = _pitch_servo, \
        .pitch_min = _pitch_min, \
        .pitch_max = _pitch_max \
}

#define DEF_FRAME(_name, _num_motors, _motors) {\
        .name = _name, \
        .num_motors = _num_motors, \
        .motors = _motors \
}
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void BaroSimpleAtmosphere(const float alt, float* sigma, float* delta, float* theta);
/*----------------------------------variable----------------------------------*/
static struct sim_motor quad_plus_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
    DEF_MOTOR(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
    DEF_MOTOR(AP_MOTORS_MOT_3,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3),
};

static struct sim_motor quad_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_3,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
};

// motor order to match betaflight conventions
// See: https://fpvfrenzy.com/betaflight-motor-order/
static struct sim_motor quad_bf_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
    DEF_MOTOR(AP_MOTORS_MOT_2,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,1),
    DEF_MOTOR(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,3),
    DEF_MOTOR(AP_MOTORS_MOT_4,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
};

// motor order to match betaflight conventions, reversed direction
static struct sim_motor quad_bf_x_rev_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
    DEF_MOTOR(AP_MOTORS_MOT_2,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3),
    DEF_MOTOR(AP_MOTORS_MOT_4,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
};

// motor order to match DJI conventions
// See: https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
static struct sim_motor quad_dji_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
};

// motor order so that test order matches motor order ("clockwise X")
static struct sim_motor quad_cw_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
    DEF_MOTOR(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_4,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
};

static struct sim_motor tiltquad_h_vectored_motors[] =
{
    DEF_MOTOR2(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1, -1, 0, 0, 7, 10, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3, -1, 0, 0, 8, 10, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_3,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4, -1, 0, 0, 8, 10, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, -1, 0, 0, 7, 10, -90),
};

static struct sim_motor hexa_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
    DEF_MOTOR(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5),
    DEF_MOTOR(AP_MOTORS_MOT_4,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
    DEF_MOTOR(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
    DEF_MOTOR(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3)
};

static struct sim_motor hexax_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
    DEF_MOTOR(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6),
    DEF_MOTOR(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_5,  30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4)
};

static struct sim_motor hexa_dji_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6),
    DEF_MOTOR(AP_MOTORS_MOT_3,  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_4, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_5,  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_6,   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2)
};

static struct sim_motor hexa_cw_x_motors[] = 
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
    DEF_MOTOR(AP_MOTORS_MOT_3,  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_4, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_5,  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_6,  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6)
};

static struct sim_motor octa_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,    0,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  180,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5),
    DEF_MOTOR(AP_MOTORS_MOT_3,   45,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
    DEF_MOTOR(AP_MOTORS_MOT_4,  135,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
    DEF_MOTOR(AP_MOTORS_MOT_5,  -45,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
    DEF_MOTOR(AP_MOTORS_MOT_6, -135,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
    DEF_MOTOR(AP_MOTORS_MOT_7,  -90,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7),
    DEF_MOTOR(AP_MOTORS_MOT_8,   90,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3)
};

static struct sim_motor octa_dji_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  8),
    DEF_MOTOR(AP_MOTORS_MOT_3,  -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
    DEF_MOTOR(AP_MOTORS_MOT_4, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6),
    DEF_MOTOR(AP_MOTORS_MOT_5, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_6,  157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_7,  112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_8,   67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2)
};

static struct sim_motor octa_cw_x_motors[] = 
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,   67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
    DEF_MOTOR(AP_MOTORS_MOT_3,  112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_4,  157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_5, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6),
    DEF_MOTOR(AP_MOTORS_MOT_7,  -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
    DEF_MOTOR(AP_MOTORS_MOT_8,  -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  8)
};

static struct sim_motor octa_quad_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7),
    DEF_MOTOR(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3),
    DEF_MOTOR(AP_MOTORS_MOT_5,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
    DEF_MOTOR(AP_MOTORS_MOT_6,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
    DEF_MOTOR(AP_MOTORS_MOT_7,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
    DEF_MOTOR(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6)
};

static struct sim_motor octa_quad_cw_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
    DEF_MOTOR(AP_MOTORS_MOT_3,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_5, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
    DEF_MOTOR(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6),
    DEF_MOTOR(AP_MOTORS_MOT_7,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
    DEF_MOTOR(AP_MOTORS_MOT_8,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  8)
};

static struct sim_motor dodeca_hexa_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_2,   30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2),
    DEF_MOTOR(AP_MOTORS_MOT_3,   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3),
    DEF_MOTOR(AP_MOTORS_MOT_4,   90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_5,  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5),
    DEF_MOTOR(AP_MOTORS_MOT_6,  150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6),
    DEF_MOTOR(AP_MOTORS_MOT_7, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7),
    DEF_MOTOR(AP_MOTORS_MOT_8, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8),
    DEF_MOTOR(AP_MOTORS_MOT_9,  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9),
    DEF_MOTOR(AP_MOTORS_MOT_10, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   10),
    DEF_MOTOR(AP_MOTORS_MOT_11, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   11),
    DEF_MOTOR(AP_MOTORS_MOT_12, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  12)
};

static struct sim_motor deca_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,     0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_2,    36, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2),
    DEF_MOTOR(AP_MOTORS_MOT_3,    72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3),
    DEF_MOTOR(AP_MOTORS_MOT_4,   108, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4),
    DEF_MOTOR(AP_MOTORS_MOT_5,   144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5),
    DEF_MOTOR(AP_MOTORS_MOT_6,   180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6),
    DEF_MOTOR(AP_MOTORS_MOT_7,  -144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7),
    DEF_MOTOR(AP_MOTORS_MOT_8,  -108, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   8),
    DEF_MOTOR(AP_MOTORS_MOT_9,   -72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9),
    DEF_MOTOR(AP_MOTORS_MOT_10,  -36, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  10)
};

static struct sim_motor deca_cw_x_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,    18, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_2,    54, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2),
    DEF_MOTOR(AP_MOTORS_MOT_3,    90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3),
    DEF_MOTOR(AP_MOTORS_MOT_4,   126, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4),
    DEF_MOTOR(AP_MOTORS_MOT_5,   162, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5),
    DEF_MOTOR(AP_MOTORS_MOT_6,  -162, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6),
    DEF_MOTOR(AP_MOTORS_MOT_7,  -126, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7),
    DEF_MOTOR(AP_MOTORS_MOT_8,   -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   8),
    DEF_MOTOR(AP_MOTORS_MOT_9,   -54, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9),
    DEF_MOTOR(AP_MOTORS_MOT_10,  -18, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  10)
};

static struct sim_motor tri_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,   60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
    DEF_MOTOR(AP_MOTORS_MOT_2,  -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
    DEF_MOTOR2(AP_MOTORS_MOT_4,  180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, AP_MOTORS_MOT_7, 60, -60, -1, 0, 0),
};

static struct sim_motor tilttri_motors[] =
{
    DEF_MOTOR2(AP_MOTORS_MOT_1,   60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, AP_MOTORS_MOT_8, 0, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_2,  -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3, -1, 0, 0, AP_MOTORS_MOT_8, 0, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_4,  180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, AP_MOTORS_MOT_7, 60, -60, -1, 0, 0),
};

static struct sim_motor tilttri_vectored_motors[] =
{
    DEF_MOTOR2(AP_MOTORS_MOT_1,   60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 7, 10, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_2,  -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3, -1, 0, 0, 8, 10, -90),
    DEF_MOTOR(AP_MOTORS_MOT_4,  180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2)
};

static struct sim_motor y6_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
    DEF_MOTOR(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5),
    DEF_MOTOR(AP_MOTORS_MOT_3, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
    DEF_MOTOR(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR(AP_MOTORS_MOT_5,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
    DEF_MOTOR(AP_MOTORS_MOT_6, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3)
};

/*
  FireflyY6 is a Y6 with front motors tiltable using servo on channel 9 (output 8)
 */
static struct sim_motor firefly_motors[] =
{
    DEF_MOTOR(AP_MOTORS_MOT_1, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
    DEF_MOTOR2(AP_MOTORS_MOT_2,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 6, 0, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_3, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5, -1, 0, 0, 6, 0, -90),
    DEF_MOTOR(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
    DEF_MOTOR2(AP_MOTORS_MOT_5,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2, -1, 0, 0, 6, 0, -90),
    DEF_MOTOR2(AP_MOTORS_MOT_6, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6, -1, 0, 0, 6, 0, -90)
};

static struct FrameModel default_model = {
        // model mass kg
        .mass = 3.0f,

        // diameter of model
        .diagonal_size = 0.35f,

        /*
          the ref values are for a test at fixed angle, used to estimate drag
         */
        .refSpd = 15.08f, // m/s
        .refAngle = 45.0f,  // degrees
        .refVoltage = 12.09f, // Volts
        .refCurrent = 29.3f, // Amps
        .refAlt = 593.0f, // altitude AMSL
        .refTempC = 25.0f, // temperature C
        .refBatRes = 0.01f, // BAT.Res

        // full pack voltage
        .maxVoltage = 4.2f*3,

        // battery capacity in Ah. Use zero for unlimited
        .battCapacityAh = 0.0f,

        // CTUN.ThO at hover at refAlt
        .hoverThrOut = 0.39f,

        // MOT_THST_EXPO
        .propExpo = 0.65f,

        // scaling factor for yaw response, deg/sec
        .refRotRate = 120.0f,

        // MOT params are from the reference test
        // MOT_PWM_MIN
        .pwmMin = 1000.0f,
        // MOT_PWM_MAX
        .pwmMax = 2000.0f,
        // MOT_SPIN_MIN
        .spin_min = 0.15f,
        // MOT_SPIN_MAX
        .spin_max = 0.95f,

        // maximum slew rate of motors
        .slew_max = 150.0f,

        // rotor disc area in m**2 for 4 x 0.35m dia rotors
        // Note that coaxial rotors count as one rotor only when calculating effective disc area
        .disc_area = 0.385f,

        // momentum drag coefficient
        .mdrag_coef = 0.2f,

        // number of motors
        .num_motors = 4.0f
};

/*
  table of supported frame types. String order is important for
  partial name matching
 */
static struct sim_frame supported_frames[] =
{
    DEF_FRAME("+",         4, quad_plus_motors),
    DEF_FRAME("quad",      4, quad_plus_motors),
    DEF_FRAME("copter",    4, quad_plus_motors),
    DEF_FRAME("x",         4, quad_x_motors),
    DEF_FRAME("bfxrev",    4, quad_bf_x_rev_motors),
    DEF_FRAME("bfx",       4, quad_bf_x_motors),
    DEF_FRAME("djix",      4, quad_dji_x_motors),
    DEF_FRAME("cwx",       4, quad_cw_x_motors),
    DEF_FRAME("tilthvec",  4, tiltquad_h_vectored_motors),
    DEF_FRAME("hexax",     6, hexax_motors),
    DEF_FRAME("hexa-cwx",  6, hexa_cw_x_motors),
    DEF_FRAME("hexa-dji",  6, hexa_dji_x_motors),
    DEF_FRAME("hexa",      6, hexa_motors),
    DEF_FRAME("octa-cwx",  8, octa_cw_x_motors),
    DEF_FRAME("octa-dji",  8, octa_dji_x_motors),
    DEF_FRAME("octa-quad-cwx",8, octa_quad_cw_x_motors),
    DEF_FRAME("octa-quad", 8, octa_quad_motors),
    DEF_FRAME("octa",      8, octa_motors),
    DEF_FRAME("deca",     10, deca_motors),
    DEF_FRAME("deca-cwx", 10, deca_cw_x_motors),
    DEF_FRAME("dodeca-hexa", 12, dodeca_hexa_motors),
    DEF_FRAME("tri",       3, tri_motors),
    DEF_FRAME("tilttrivec",3, tilttri_vectored_motors),
    DEF_FRAME("tilttri",   3, tilttri_motors),
    DEF_FRAME("y6",        6, y6_motors),
    DEF_FRAME("firefly",   6, firefly_motors)
};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  find a frame by name
 */
sim_frame_t sim_frame_find_frame(const char *name)
{
    for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) {
        // do partial name matching to allow for frame variants
        if (strncmp(name, supported_frames[i].name, strlen(supported_frames[i].name)) == 0) {
            return &supported_frames[i];
        }
    }

    return NULL;
}

/*
  initialise the frame
 */
void sim_frame_init(sim_frame_t frame, const char *frame_str, sim_battery_t _battery)
{
    frame->model = default_model;
    frame->battery = _battery;

    frame->mass = frame->model.mass;

    const float drag_force = frame->model.mass * GRAVITY_MSS * tanf(radians(frame->model.refAngle));

    const float cos_tilt = cosf(radians(frame->model.refAngle));
    const float airspeed_bf = frame->model.refSpd * cos_tilt;
    const float ref_thrust = frame->model.mass * GRAVITY_MSS / cos_tilt;
    float ref_air_density = sim_frame_get_air_density(frame, frame->model.refAlt);

    const float momentum_drag = cos_tilt * frame->model.mdrag_coef * airspeed_bf * sqrtf(ref_thrust * ref_air_density * frame->model.disc_area);

    if (momentum_drag > drag_force) {
        frame->model.mdrag_coef *= drag_force / momentum_drag;
        frame->areaCd = 0.0;
        printf("Suggested EK3_DRAG_BCOEF_* = 0, EK3_DRAG_MCOEF = %.3f\n", (momentum_drag / (frame->model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density));
    } else {
        frame->areaCd = (drag_force - momentum_drag) / (0.5f * ref_air_density * sq(frame->model.refSpd));
        printf("Suggested EK3_DRAG_BCOEF_* = %.3f, EK3_DRAG_MCOEF = %.3f\n", frame->model.mass / frame->areaCd, (momentum_drag / (frame->model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density));
    }

    frame->terminal_rotation_rate = frame->model.refRotRate;

    float hover_thrust = frame->mass * GRAVITY_MSS;
    float hover_power = frame->model.refCurrent * frame->model.refVoltage;
    float hover_velocity_out = 2 * hover_power / hover_thrust;
    float effective_disc_area = hover_thrust / (0.5f * ref_air_density * sq(hover_velocity_out));
    float velocity_max = hover_velocity_out / sqrtf(frame->model.hoverThrOut);
    float effective_prop_area = effective_disc_area / frame->num_motors;
    float true_prop_area = frame->model.disc_area / frame->num_motors;

    // power_factor is ratio of power consumed per newton of thrust
    float power_factor = hover_power / hover_thrust;

    sim_battery_setup(frame->battery, frame->model.battCapacityAh, frame->model.refBatRes, frame->model.maxVoltage);

    if ((uint8_t)(frame->model.num_motors) != frame->num_motors) {
        printf("Warning model expected %u motors and got %u\n", (uint8_t)(frame->model.num_motors), frame->num_motors);
    }

    for (uint8_t i=0; i<frame->num_motors; i++) {
        sim_motor_init(&frame->motors[i]);
        sim_motor_setup_params(&frame->motors[i], (uint16_t)frame->model.pwmMin, (uint16_t)frame->model.pwmMax, frame->model.spin_min, frame->model.spin_max, frame->model.propExpo, frame->model.slew_max,
                               frame->model.diagonal_size, power_factor, frame->model.maxVoltage, effective_prop_area, velocity_max,
                               &frame->model.motor_pos[i], &frame->model.motor_thrust_vec[i], frame->model.yaw_factor[i], true_prop_area,
                               frame->model.mdrag_coef);
    }

    if (math_flt_zero(frame->model.moment_of_inertia.x) || math_flt_zero(frame->model.moment_of_inertia.y) || math_flt_zero(frame->model.moment_of_inertia.z)) {
        // if no inertia provided, assume 50% of mass on ring around center
        frame->model.moment_of_inertia.x = frame->model.mass * 0.25f * sq(frame->model.diagonal_size*0.5f);
        frame->model.moment_of_inertia.y = frame->model.moment_of_inertia.x;
        frame->model.moment_of_inertia.z = frame->model.mass * 0.5f * sq(frame->model.diagonal_size*0.5f);
    }

    // setup reasonable defaults for battery
    param_t* p = param_get_by_full_name("SIM", "SIM_BATT_VOLTAGE");
    if (p != NULL) {
        param_set_float(p, frame->model.maxVoltage);
    }

    p = param_get_by_full_name("SIM", "SIM_BATT_CAP_AH");
    if (p != NULL) {
        param_set_float(p, frame->model.battCapacityAh);
    }

    if (frame->model.battCapacityAh > 0) {
        p = param_get_by_full_name("SIM", "BATT_CAPACITY");
        if (p != NULL) {
            param_set_float(p, frame->model.battCapacityAh*1000);
        }
    }
}

// get air density in kg/m^3
float sim_frame_get_air_density(sim_frame_const_t frame, float alt_amsl)
{
    float sigma, delta, theta;

    BaroSimpleAtmosphere(alt_amsl * 0.001f, &sigma, &delta, &theta);

    const float air_pressure = SSL_AIR_PRESSURE * delta;
    return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN * frame->model.refTempC));
}

// calculate rotational and linear accelerations
void sim_frame_calculate_forces(sim_frame_t frame, sim_aircraft_const_t aircraft,
                             const struct sitl_input *input,
                             Vector3f_t *rot_accel,
                             Vector3f_t *body_accel,
                             float* rpm,
                             bool use_drag)
{
    Vector3f_t thrust = VECTOR3F_DEFAULT_VALUE; // newtons
    Vector3f_t torque = VECTOR3F_DEFAULT_VALUE;

    const float air_density = sim_frame_get_air_density(frame, sim_aircraft_get_location(aircraft)->alt*0.01f);
    const Vector3f_t* gyro = sim_aircraft_get_gyro(aircraft);

    Vector3f_t vel_air_bf;
    matrix3f_mul_transpose_vec(&vel_air_bf, sim_aircraft_get_dcm(aircraft), sim_aircraft_get_velocity_air_ef(aircraft));

    float current = 0;
    for (uint8_t i=0; i<frame->num_motors; i++) {
        Vector3f_t mtorque = VECTOR3F_DEFAULT_VALUE;
        Vector3f_t mthrust = VECTOR3F_DEFAULT_VALUE;
        sim_motor_calculate_forces(&frame->motors[i], input, frame->motor_offset, &mtorque, &mthrust, &vel_air_bf, gyro, air_density, sim_battery_get_voltage(frame->battery), use_drag);
        current += sim_motor_get_current(&frame->motors[i]);

        torque.x += mtorque.x;
        torque.y += mtorque.y;
        torque.z += mtorque.z;

        thrust.x += mthrust.x;
        thrust.y += mthrust.y;
        thrust.z += mthrust.z;

        // simulate motor rpm
        if (!math_flt_zero(aircraft->sitl->vibe_motor)) {
            rpm[frame->motor_offset+i] = sim_motor_get_command(&frame->motors[i]) * aircraft->sitl->vibe_motor * 60.0f;
        }
    }

    // calculate total rotational acceleration
    rot_accel->x = torque.x / frame->model.moment_of_inertia.x;
    rot_accel->y = torque.y / frame->model.moment_of_inertia.y;
    rot_accel->z = torque.z / frame->model.moment_of_inertia.z;

    if (frame->terminal_rotation_rate > 0) {
        // rotational air resistance
        rot_accel->x -= gyro->x * radians(400.0f) / frame->terminal_rotation_rate;
        rot_accel->y -= gyro->y * radians(400.0f) / frame->terminal_rotation_rate;
        rot_accel->z -= gyro->z * radians(400.0f) / frame->terminal_rotation_rate;
    }

    if (use_drag) {
        // use the model params to calculate drag
        Vector3f_t drag_bf;
        drag_bf.x = frame->areaCd * 0.5f * air_density * sq(vel_air_bf.x);
        if (math_flt_negative(vel_air_bf.x)) {
            drag_bf.x = -drag_bf.x;
        }

        drag_bf.y = frame->areaCd * 0.5f * air_density * sq(vel_air_bf.y);
        if (math_flt_negative(vel_air_bf.y)) {
            drag_bf.y = -drag_bf.y;
        }

        drag_bf.z = frame->areaCd * 0.5f * air_density * sq(vel_air_bf.z);
        if (math_flt_negative(vel_air_bf.z)) {
            drag_bf.z = -drag_bf.z;
        }

        thrust.x -= drag_bf.x;
        thrust.y -= drag_bf.y;
        thrust.z -= drag_bf.z;
    }

    body_accel->x = thrust.x/sim_aircraft_gross_mass(aircraft);
    body_accel->y = thrust.y/sim_aircraft_gross_mass(aircraft);
    body_accel->z = thrust.z/sim_aircraft_gross_mass(aircraft);
}

// calculate current and voltage
void sim_frame_current_and_voltage(sim_frame_t frame, float *voltage, float *current)
{
    float param_voltage = sim_get_singleton()->batt_voltage;
    if (!math_flt_equal(frame->last_param_voltage,param_voltage)) {
        sim_battery_init_voltage(frame->battery, param_voltage);
        frame->last_param_voltage = param_voltage;
    }
    *voltage = sim_battery_get_voltage(frame->battery);
    *current = 0;
    for (uint8_t i=0; i<frame->num_motors; i++) {
        (*current) += sim_motor_get_current(&frame->motors[i]);
    }
}

/* 
   Compute the temperature, density, and pressure in the standard atmosphere
   Correct to 20 km.  Only approximate thereafter.
*/
static void BaroSimpleAtmosphere(
    const float alt,                           // geometric altitude, km.
    float* sigma,                   // density/sea-level standard density
    float* delta,                 // pressure/sea-level standard pressure
    float* theta)           // temperature/sea-level standard temperature
{
    const float REARTH = 6369.0f;        // radius of the Earth (km)
    const float GMR    = 34.163195f;     // gas constant
    float h=alt*REARTH/(alt+REARTH);     // geometric to geopotential altitude

    if (h < 11.0f) {
        // Troposphere
        *theta = (SSL_AIR_TEMPERATURE - 6.5f * h) / SSL_AIR_TEMPERATURE;
        *delta = powf(*theta, GMR / 6.5f);
    } else {
        // Stratosphere
        *theta = 216.65f / SSL_AIR_TEMPERATURE;
        *delta = 0.2233611f * expf(-GMR * (h - 11.0f) / 216.65f);
    }

    *sigma = (*delta)/(*theta);
}

/*------------------------------------test------------------------------------*/


